package gnss

import (
	"dgo/goutils/utils"
	"encoding/binary"
	"fmt"
	"math"
	"time"
)

/*
**

	Bin16Message length = 8 + (8+2+2+4+4+4+192+64+32=312) + 2 + 2 = 324
*/
type HemiBin16Message struct {
	Tow      float64
	Week     uint16
	PageInfo uint32

	// bit 0 = GPS:L1CA included
	// bit 1 = GPS:L2P included
	// bit 2 = GPS:L2C included
	// bit 3 = GPS:L5 included
	// bit 7:4 = spare
	// bit 8 = GLO:G1C or GLO:G1P included
	// bit 9 = GLO:G2C or GLO:G1P included
	// bit 15:10 = spare
	// bit 16 = GAL:E1BC included
	// bit 17 = GAL:E5A included
	// bit 18 = GAL:E5B included
	// bit 23:19 = spare
	// bit 24 = BDS:B1I included
	// bit 25 = BDS:B2I included
	// bit 26 = BDS:B3I included
	// bit 31:27 = spare
	SignalBits1 uint32

	// bit 0 = QZS:L1CA included
	// bit 1 = spare
	// bit 2 = QZS:L2C included
	// bit 3 = QZS:L5 included
	// bit 4 = QZS:L1C included
	// bit 31:5 = spare
	SignalBits2 uint32

	ObsLst [16]HemiObsPacket

	// array of 16, 32 bit words   (16*4=64 bytes)
	// bit 7:0 (8 bits) = satellite PRN,
	//                  = 0 if no satellite
	// bit 12:8 (5 bits) = Log_Base_2(X+1)
	//        where X = Time, in units of 1/100th sec,
	//        since carrier phase tracking was last stressed
	//        or cycle slipped
	// bit 31:13 (19 bits) = upper 19 bits
	// of code pseudorange LSB = 256 meters
	//                     MSB = 67108864 meters
	CodeMSBsPRN [16]uint32

	// Array of 16, 16 bit words (32 bytes)
	//[15,14]  spare bits
	//[13] = 1 if GLONASS P-Code
	//[12,11,10,9,8] = Channel (0 is the first channel)
	//[7,6,5,4] = Signal ID (L1CA, L5, G1, B1I, B2I, B3I, etc)
	// GPS Signal ID: L1CA=0, L2P=1, L2C=2, L5=3
	// GLO Signal ID: G1C/G1P=0, G2C/G2P=1, G10C=4, G20C=5, G30C=6
	// GAL Signal ID: E1BC=0, E5A=1, E5B=2, E6=3, ALTBOC=4
	// BDS Signal ID: B1I=0, B2I=1, B3I=2,B1BOC=3,B2A=4,B2B=5,B3C=6,ACEBOC=7
	// QZS Signal ID: L1CA=0, L2C=2, L5=3, L1C=4
	//[3,2,1,0] = GNSS System, 0=GPS,1=GLO,2=GAL,3=BDS,4=QZS
	ChanSignalSYS [16]uint16
}

// 获取伪距
func (this *HemiBin16Message) GetPsRng(idx int) float64 {
	// 伪距的 高19位
	v0 := this.CodeMSBsPRN[idx]
	v0 = v0 >> 13 // 13-31

	vl := this.ObsLst[idx].GetPsRng()
	return float64(v0*256) + vl
}

func (this *HemiBin16Message) GetPhRng(idx int) float64 {
	v1 := this.ObsLst[idx].GetPhRng()
	sigtype := this.GetSignalType(idx)

	wl := GetWaveLength(this.GetNaviSys(idx), sigtype, this.GetSatID(idx))
	if wl == 0 {
		panic(fmt.Sprintf("未知的波长[%s][%d]", GetPrnStrCode(this.GetSatID(idx), this.GetNaviSys(idx)), idx))
	}
	psrng := this.GetPsRng(idx)
	ncycle := psrng / wl
	ncycle0 := int64(ncycle) >> 13 << 13
	rval := float64(ncycle0) + v1
	span := rval - ncycle
	if span > 4096 {
		rval -= 8192
	} else if span < -4096 {
		rval += 8192
	}
	//v2 := this.CodeMSBsPRN[idx] << 19 >> 27;
	//vl := math.Log2(float64(v2) +1)
	// fmt.Printf("\t\t%.4f %d %.4f\n", ncycle, ncycle0, v1);
	return rval
}

func (this *HemiBin16Message) GetSNR(idx int) float32 {
	snr := this.ObsLst[idx].GetRawSNR()
	if snr == 0 {
		return 0
	}

	naviSys := this.GetNaviSys(idx)
	if naviSys == SYS_GPS {
		sig := this.GetSignal(idx)
		if sig == 1 || sig == 2 {
			return float32(10 * math.Log10(snr*0.1164*1000))
		}
	}
	snr = 10 * math.Log10(snr*0.1024*1000)
	return float32(snr)
}

/*
**

	那是在后面输出到Rinex格式的时候统一乘以-1了，您那边解析的时候先按照H板文档进行解析吧
*/
func (this *HemiBin16Message) GetDoppler(idx int) float64 {
	return this.ObsLst[idx].GetDoppler()
}

func (this *HemiBin16Message) DecodeTime() time.Time {
	// 考虑2HZ的数据
	return TOW2TimeEx(uint(this.Week), uint((this.Tow+0.001)*1000))
}

func (this *HemiBin16Message) GetTrackTime(idx int) float64 {
	return this.ObsLst[idx].GetTrackTime()
}

func (this *HemiBin16Message) WriteGnssSV(epoch *GnssSVEpoch, onSignal Func_signconvert, checkfn func(navisys byte, satid uint16, gnsssvsigId byte) bool) {

	for i := 0; i < 16; i++ {
		naviSys := this.GetNaviSys(i)
		satid := this.GetSatID(i)
		//if naviSys == SYS_BD && satid == 23 {
		//	satid = 23
		//}
		if satid > 0 {
			if satid > 100 {
				if naviSys == SYS_GPS {
					naviSys = SYS_SBS
					satid -= 100
				} else if naviSys == SYS_QZS {
					satid -= 192
				} else {
					satid -= 100
				}
			}

			sigid := this.GetSignal(i)

			var gnsssvsigId byte
			if onSignal != nil {
				gnsssvsigId = onSignal(naviSys, sigid)
			}
			if gnsssvsigId == 0 {
				sigex := this.GetSignalEx(i)
				gnsssvsigId = HemiSigId2GnssSVSigId(naviSys, sigex, sigid)
			}

			if gnsssvsigId > 0 {
				if checkfn == nil || checkfn(naviSys, uint16(satid), gnsssvsigId) {
					if satid != 0 {
						idx := gnsssvsigId - 1
						obs := epoch.CheckGetRawObs(naviSys, uint16(satid))
						obs.PsRngV[idx] = this.GetPsRng(i)
						obs.PhRngV[idx] = this.GetPhRng(i)
						obs.Doppler[idx] = -this.GetDoppler(i)
						obs.Cnr[idx] = this.GetSNR(i)
						obs.LkT[idx] = int64(this.GetTrackTime(i) * 1000)
					}
				}
			}
		}
	}
}

func (this *HemiBin16Message) WriteYObsEx(yobsEpoch *GnssYObsEpoch, checkfn func(navisys, satid, ysigid byte) bool) {
	for i := 0; i < 16; i++ {
		naviSys := this.GetNaviSys(i)
		satid := this.GetSatID(i)
		//if naviSys == SYS_BD && satid == 23 {
		//	satid = 23
		//}
		if satid > 0 {
			if satid > 100 {
				if naviSys == SYS_GPS {
					naviSys = SYS_SBS
					satid -= 100
				} else if naviSys == SYS_QZS {
					satid -= 192
				} else {
					satid -= 100
				}
			}

			sigid := this.GetYObsSigID(i)
			//if sigid == YOBS_BDS_SIGN_B1C_1P && naviSys == SYS_BD {
			//	sigid = YOBS_BDS_SIGN_B1C_1P
			//}
			if sigid > 0 {
				if checkfn == nil || checkfn(naviSys, satid, sigid) {
					if satid != 0 {
						sigid = sigid - 1
						obs := yobsEpoch.CheckGetRawObs(naviSys, satid)
						obs.PsRngV[sigid] = this.GetPsRng(i)
						obs.PhRngV[sigid] = this.GetPhRng(i)
						obs.Doppler[sigid] = -this.GetDoppler(i)
						obs.Cnr[sigid] = this.GetSNR(i)
						obs.LkT[sigid] = int64(this.GetTrackTime(i) * 1000)
					}
				}
			}
		}
	}
}

func (this *HemiBin16Message) WriteYObs(yobsEpoch *GnssYObsEpoch) {
	this.WriteYObsEx(yobsEpoch, nil)
}

func (this *HemiBin16Message) GetSatID(idx int) byte {
	v := this.CodeMSBsPRN[idx]
	l := uint16(32 - 8)
	v = v << l >> l
	return byte(v)
}

/*
*

	//[7,6,5,4] = Signal ID (L1CA, L5, G1, B1I, B2I, B3I, etc)
	// GPS Signal ID: L1CA=0, L2P=1, L2C=2, L5=3
	// GLO Signal ID: G1C/G1P=0, G2C/G2P=1, G10C=4, G20C=5, G30C=6
	// GAL Signal ID: E1BC=0, E5A=1, E5B=2, E6=3, ALTBOC=4
	// BDS Signal ID: B1I=0, B2I=1, B3I=2,B1BOC=3,B2A=4,B2B=5,B3C=6,ACEBOC=7
	// QZS Signal ID: L1CA=0, L2C=2, L5=3, L1C=4
*/
func (this *HemiBin16Message) GetSignal(idx int) byte {
	v := this.ChanSignalSYS[idx]
	l := uint16(16 - 8)
	v = v << l >> (l + 4)
	return byte(v)
}

// [13] = 1 if GLONASS P-Code
func (this *HemiBin16Message) GetSignalEx(idx int) byte {
	v := this.ChanSignalSYS[idx]
	return utils.GetBitValue0(uint64(v), 13)
}

/*
*
// Array of 16, 16 bit words (32 bytes)

	//[15,14]  spare bits
	//[13] = 1 if GLONASS P-Code
	//[12,11,10,9,8] = Channel (0 is the first channel)
	//[7,6,5,4] = Signal ID (L1CA, L5, G1, B1I, B2I, B3I, etc)
	// GPS Signal ID: L1CA=0, L2P=1, L2C=2, L5=3
	// GLO Signal ID: G1C/G1P=0, G2C/G2P=1, G10C=4, G20C=5, G30C=6
	// GAL Signal ID: E1BC=0, E5A=1, E5B=2, E6=3, ALTBOC=4
	// BDS Signal ID: B1I=0, B2I=1, B3I=2,B1BOC=3,B2A=4,B2B=5,B3C=6,ACEBOC=7
	// QZS Signal ID: L1CA=0, L2C=2, L5=3, L1C=4
	//[3,2,1,0] = GNSS System, 0=GPS,1=GLO,2=GAL,3=BDS,4=QZS
*/
func (this *HemiBin16Message) GetYObsSigID(idx int) (sigid byte) {
	naviSys := this.GetNaviSys(idx)
	sig := this.GetSignal(idx)
	if naviSys == SYS_GPS || naviSys == SYS_SBS {
		if int(sig) < len(HEMI_GPS_SIGN_YOBS_MAP) {
			sigid = HEMI_GPS_SIGN_YOBS_MAP[sig]
		}
	} else if naviSys == SYS_GLO {
		if sig == 0 {
			if this.GetSignalEx(idx) == 1 { // 1 if GLONASS P-Code
				return YOBS_GLO_SIGN_G1_1P
			} else {
				return YOBS_GLO_SIGN_G1_1C
			}
		} else if sig == 1 {
			if this.GetSignalEx(idx) == 1 { // 1 if GLONASS P-Code
				return YOBS_GLO_SIGN_G2_2P
			} else {
				return YOBS_GLO_SIGN_G2_2C
			}
		}

		if int(sig) < len(HEMI_GLO_SIGN_YOBS_MAP) {
			sigid = HEMI_GLO_SIGN_YOBS_MAP[sig]
		}
	} else if naviSys == SYS_GAL {
		if int(sig) < len(HEMI_GAL_SIGN_YOBS_MAP) {
			sigid = HEMI_GAL_SIGN_YOBS_MAP[sig]
		}
	} else if naviSys == SYS_QZS {
		if int(sig) < len(HEMI_QZS_SIGN_YOBS_MAP) {
			sigid = HEMI_QZS_SIGN_YOBS_MAP[sig]
		}
	} else if naviSys == SYS_BD {
		if int(sig) < len(HEMI_BDS_SIGN_YOBS_MAP) {
			sigid = HEMI_BDS_SIGN_YOBS_MAP[sig]
		}

	}

	return
}

/*
*

	//[7,6,5,4] = Signal ID (L1CA, L5, G1, B1I, B2I, B3I, etc)
	// GPS Signal ID: L1CA=0, L2P=1, L2C=2, L5=3
	// GLO Signal ID: G1C/G1P=0, G2C/G2P=1, G10C=4, G20C=5, G30C=6
	// GAL Signal ID: E1BC=0, E5A=1, E5B=2, E6=3, ALTBOC=4
	// BDS Signal ID: B1I=0, B2I=1, B3I=2,B1BOC=3,B2A=4,B2B=5,B3C=6,ACEBOC=7
	// QZS Signal ID: L1CA=0, L2C=2, L5=3, L1C=4
*/
func (this *HemiBin16Message) GetSignalType(idx int) (sigtype byte) {
	naviSys := this.GetNaviSys(idx)
	sig := this.GetSignal(idx)

	if naviSys == SYS_GPS || naviSys == SYS_SBS {
		if sig == 0 {
			sigtype = 1
		} else if sig == 1 || sig == 2 {
			sigtype = 2
		} else if sig == 3 {
			sigtype = 5
		}
	} else if naviSys == SYS_GLO {
		if int(sig) < len(HEMI_GLO_SIGTYPE_YOBS_MAP) {
			return HEMI_GLO_SIGTYPE_YOBS_MAP[sig]
		}
	} else if naviSys == SYS_GAL {
		if int(sig) < len(HEMI_GLA_SIGTYPE_YOBS_MAP) {
			return HEMI_GLA_SIGTYPE_YOBS_MAP[sig]
		}
	} else if naviSys == SYS_BD {
		if int(sig) < len(HEMI_BDS_SIGTYPE_YOBS_MAP) {
			return HEMI_BDS_SIGTYPE_YOBS_MAP[sig]
		}
	} else if naviSys == SYS_QZS {
		if int(sig) < len(HEMI_QZS_SIGTYPE_YOBS_MAP) {
			return HEMI_QZS_SIGTYPE_YOBS_MAP[sig]
		}
	}

	return
}

func (this *HemiBin16Message) GetNaviSys(idx int) byte {
	v := this.ChanSignalSYS[idx]
	l := uint16(16 - 4)
	v = v << l >> l
	switch v {
	case 0:
		return SYS_GPS
	case 1:
		return SYS_GLO
	case 2:
		return SYS_GAL
	case 3:
		return SYS_BD
	case 4:
		return SYS_QZS
	default:
		return 0
	}
}

func (this *HemiBin16Message) Decode(packBuf []byte) error {
	byteN := 8

	bits := binary.LittleEndian.Uint64(packBuf[byteN:])
	byteN += 8

	this.Tow = math.Float64frombits(bits)
	this.Week = binary.LittleEndian.Uint16(packBuf[byteN:])
	byteN += 2

	byteN += 2

	this.PageInfo = binary.LittleEndian.Uint32(packBuf[byteN:])
	byteN += 4

	this.SignalBits1 = binary.LittleEndian.Uint32(packBuf[byteN:])
	byteN += 4
	this.SignalBits2 = binary.LittleEndian.Uint32(packBuf[byteN:])
	byteN += 4

	for i := 0; i < len(this.ObsLst); i++ {
		this.ObsLst[i].Decode(packBuf[byteN:])
		byteN += 12
	}

	for i := 0; i < len(this.CodeMSBsPRN); i++ {
		this.CodeMSBsPRN[i] = binary.LittleEndian.Uint32(packBuf[byteN:])
		byteN += 4
	}

	for i := 0; i < len(this.ChanSignalSYS); i++ {
		this.ChanSignalSYS[i] = binary.LittleEndian.Uint16(packBuf[byteN:])
		byteN += 2
	}

	return nil
}

/*
*

	//[16,17,18,19,20,21] Number of Pages = N
	//[22,23,24,25,26,27] Page Number [0...N-1]
*/
func (this *HemiBin16Message) DecodePage() (pageIdx, pageTotal byte) {
	//pageIdx = byte(this.PageInfo << 10 >> (10 + 16))
	//pageTotal = byte(this.PageInfo << 4 >> (4 + 5 + 16))
	pageTotal = byte(utils.GetU64BitsU_L(uint64(this.PageInfo), 16, 6))
	pageIdx = byte(utils.GetU64BitsU_L(uint64(this.PageInfo), 22, 6)) + 1
	return
}

/*
*

	返回TOW, 单位是毫秒
*/
func (this *HemiBin16Message) GetTOW() uint32 {
	return uint32(math.Round(this.Tow*10) / 10 * 1000)
	//return uint32((this.Tow + 0.0005) * 1000);
}

func (this *HemiBin16Message) IsEndPage() bool {
	pageIdx, pageTotal := this.DecodePage()
	return pageTotal == pageIdx
}
